/*
 * @file grab_ball_server.cpp
 * @brief	state for grabbing the ball.
 *
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @date Jan 2013
 */

#include "servers/grab_ball_server.h"

/**
 * Constructor
 */
GrabBallAction::GrabBallAction(std::string name) : BaseServer(name),
	as_(nh_, name, boost::bind(&GrabBallAction::execute, this, _1), false)
{
	ROS_ASSERT(nh_.getParam("stop_distance", stop_distance_));
	as_.start();
}

GrabBallAction::~GrabBallAction() {
}

void GrabBallAction::execute(const rp_states::GrabBallGoalConstPtr &goal) {
	ROS_INFO("Grabbing ball!");

	// Open gripper
	ROS_INFO("Grab ball: open gripper.");
	while(getGripperState() != GRIPPER_OPEN) {
		comm::gripper msg;
		msg.state = GRIPPER_OPEN;
		publishGripper(msg);
	}

	// Wait for trunk to be down.
	ROS_INFO("Grab ball: trunk down.");
	while (getTrunkState() != TRUNK_DOWN) {
		comm::trunk msg;
		msg.state = TRUNK_DOWN;
		publishTrunk(msg);
	}

	// Move forward
	ROS_INFO("Grab ball: moving forward.");

	ros::Time sec = ros::Time::now() + ros::Duration(2.2);
	while(sec > ros::Time::now())
	{
		geometry_msgs::Twist twist;
		twist.linear.x	= 0.2;
		twist.angular.z	= 0;
		publishWheelsSpeed(twist);
	}

	// stop
	geometry_msgs::Twist twist;
	twist.linear.x	= 0.0;
	twist.angular.z	= 0;
	publishWheelsSpeed(twist);	// check the sensors
	ROS_INFO("Sensor distance: %f", getSensorDistance());
	result_.ball_obtained = getSensorDistance() < 5;

	// wait a sec
	sec = ros::Time::now() + ros::Duration(1.0);
	while(sec > ros::Time::now());

	ROS_INFO("Grab ball: closing gripper.");
	while (getGripperState() != GRIPPER_CLOSED) {
		comm::gripper msg;
		msg.state = GRIPPER_CLOSED;
		publishGripper(msg);
	}

	// Wait for trunk to be up.
	ROS_INFO("Grab ball: trunk up.");
	while (getTrunkState() != TRUNK_UP) {
		comm::trunk msg;
		msg.state = TRUNK_UP;
		publishTrunk(msg);
	}

	// check the sensors
	ROS_INFO("Sensor distance: %f", getSensorDistance());
	result_.ball_obtained = getSensorDistance() < 5;

	// TODO Write failed to pick up ball script.
	// Read data from trunk mounted sensor.
	/*
		if (ball_grab == true) {
			result_.ball_obtained = true;
		} else {
			result_.ball_obtained = false;
		}
	 */
	// open the gripper if the ball is not fetched
	if(!result_.ball_obtained)
	{
		comm::gripper msg;
		msg.state = GRIPPER_OPEN;
		publishGripper(msg);
	}

	ROS_INFO("Resultaat OPPIKKEN: %i", result_.ball_obtained);
	as_.setSucceeded(result_);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "grab_ball");

	GrabBallAction grabber(ros::this_node::getName());
	ROS_INFO("Ball grabber online.");
	ros::spin();

	return 0;
}


